home *** CD-ROM | disk | FTP | other *** search
/ Aminet 7 / Aminet 7 - August 1995.iso / Aminet / misc / sci / RARS_Amiga_3.lha / RARS / tut5.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  1995-05-27  |  7.5 KB  |  175 lines
  1. /* TUT5.CPP - yet another RARS 'bot, by M. Timin, April '95
  2. // for ver. 0.60
  3.  
  4. This file is the fifth in our tutorial series.  It is also a working
  5. robot.  The robot function name, and its displayed name, is "TutManTu".
  6. This file may be compiled and linked just as any other RARS driver.
  7. It has been tested, and it runs very well on almost all tracks.  
  8.  
  9. This code is closely based on the first four tutorials.  I will try to
  10. explain where and why it differs in the comments of the code.  There are
  11. two other files that all of our readers should get from the ftp site.
  12. They are vectors.txt and vectors.pcx.  The first is an explanation of
  13. the vector relationships used in the car model.  The second is a PCX
  14. file of an accompanying sketch.
  15.  
  16. The file CNTRL0.CPP is still useful as a reference for some details
  17. of robot code that are not explained below.
  18.  
  19. m          */
  20.  
  21. #include <string.h>
  22. #include <stdlib.h>
  23. #include <math.h>
  24. #include "car.h"
  25.  
  26. // parameters to tinker with:
  27. const double DELTA_LANE = 1.0;  // when car "dead_ahead, change "lane" this much
  28. const double CORN_MYU   =  .92;       // lateral g's expected when cornering
  29. const double BRAKE_ACCEL = -30.0;     // acceleration when braking on straight
  30. const double BRAKE_RATIO = .935;      // tire speed ratio when braking   "
  31. const double BRK_CRV_ACC = -25.0;     // acceleration when braking in curve
  32. const double BRK_CRV_SLIP = 5.0;      // tire slip for braking in curve
  33. const double DIST_FROM_INSIDE = 12.0; // target distance from curve's inner rail
  34. const double STEER_GAIN = 0.5;   // gain of steering servo loop
  35. const double  DAMP_GAIN = 1.1;        // damping of steering servo loop
  36. const double  BIG_SLIP = 9.0;     // affects the bias of steering servo loop
  37.  
  38. double corn_spd(double radius)     // returns maximum cornering speed, fps
  39. {
  40.      //  MUST NEVER CALL THIS ROUTINE WITH ZERO ARGUMENT!                                                          
  41.      return sqrt(radius * 32.2 * CORN_MYU);     // compute the speed
  42. }
  43.  
  44. // Calculates the critical distance necessary to bring a car from speed
  45. // v0 to speed v1 when the braking acceleration is "a", ft per sec^2.
  46. // Speeds are in fps.  ("a" should be negative)
  47. double CritDist(double v0, double v1, double a)
  48. {
  49.    double dv;
  50.  
  51.    dv = v1 - v0;
  52.    if(dv > 0.0)          // this saves having such a test in the caller
  53.       return(0.0);  
  54.    return (v0 + .5 * dv) * dv / a;
  55. }
  56.  
  57. con_vec TutMan3(situation &s)
  58. {
  59.    const char name[] = "TutMan3";    // This is the robot driver's name!
  60.    static int init_flag = 1;          // cleared by first call
  61.    con_vec result;                    // This is what is returned.
  62.    double alpha, vc;                  // components of result
  63.    static double lane = -10000;    // an absurd value to show not initialized
  64.    double bias, speed, speed_next, width, to_end;
  65.  
  66.    if(init_flag == 1)  {  // first time only, copy name:
  67.       my_name_is(name);        // copy the name string into the host program
  68.       init_flag = 0;
  69.       result.alpha = result.vc = 0;
  70.       return result;
  71.    }
  72.  
  73.    // service routine in the host software to handle getting unstuck from
  74.    // from crashes and pileups:
  75.    if(stuck(s.backward, s.v,s.vn, s.to_lft,s.to_rgt, &result.alpha,&result.vc))
  76.       return result;
  77.  
  78.    width = s.to_lft + s.to_rgt;   // compute width of track
  79.  
  80.    // This is a little trick so that the car will not try to change lanes
  81.    // during the "dragout" at the start of the race.  We set "lane" to
  82.    // whatever position we have been placed by the host.
  83.    if(lane < -9000)              // will be true only once
  84.       lane = s.to_lft;           // better not to change lanes at the start
  85.  
  86.    // Set "lane" during curves.  This robot sets "lane" during curves to
  87.    // try to maintain a fixed distance to the inner rail.
  88.    // For straightaways, we leave "lane" unchanged until later.
  89.    if(s.cur_rad > 0.0)                // turning left
  90.       lane = DIST_FROM_INSIDE;
  91.    else if(s.cur_rad < 0.0)           // turning right
  92.       lane = width - DIST_FROM_INSIDE;
  93.  
  94.    // set the bias:
  95.    // Bias is an additive term in the steering servo, so that the servo
  96.    // doesn't have to "hunt" much for the correct alpha value.  It is an
  97.    // estimate of the alpha value that would be found by the servo if there
  98.    // was plenty of settling time.  It is zero for straightaways.
  99.    // Also, for convenience, we call the corn_spd() function here.  On
  100.    // the straightaway, we call it to find out the correct speed for the
  101.    // corner ahead, using s.nex_rad for the radius.  In the curve we of
  102.    // course use the radius of the curve we are in.  But also, we call it
  103.    // for the next segment, to find out our target speed for the end of
  104.    // the current segment, which we call speed_next.
  105.    if(s.cur_rad == 0.0) {
  106.       bias = 0.0;
  107.       if(s.nex_rad > 0.0)
  108.          speed = corn_spd(s.nex_rad + DIST_FROM_INSIDE);
  109.       else if(s.nex_rad < 0.0)
  110.          speed = corn_spd(-s.nex_rad + DIST_FROM_INSIDE);
  111.       else
  112.          speed = 250.0;
  113.    }
  114.    else  {
  115.       if(s.nex_rad == 0.0)
  116.          speed_next = 250.0;
  117.       else
  118.          speed_next = corn_spd(fabs(s.nex_rad) + DIST_FROM_INSIDE);
  119.       speed = corn_spd(fabs(s.cur_rad) + DIST_FROM_INSIDE);
  120.       bias = (s.v*s.v/(speed*speed)) * atan(BIG_SLIP / speed);
  121.       if(s.cur_rad < 0.0)   // bias must be negative for right turn
  122.          bias = -bias;
  123.    }
  124.  
  125.    // set alpha:  (This line is the complete steering servo.)
  126.    alpha = STEER_GAIN * (s.to_lft - lane)/width - DAMP_GAIN * s.vn/s.v + bias;
  127.  
  128.    // set vc:  When nearing end of straight, change "lane" for the turn, also.
  129.    if(s.cur_rad == 0.0) {             // If we are on a straightaway,
  130.                                               // if we are far from the end,
  131.       if(s.to_end > CritDist(s.v, speed, BRAKE_ACCEL))
  132.          vc = s.v + 50.0;                           // pedal to the metal!
  133.       else  {                    // otherwise, adjust speed for the coming turn:
  134.          if(s.v > 1.02 * speed)             // if we're 2% too fast,
  135.             vc = BRAKE_RATIO * s.v;         // brake hard.
  136.          else if(s.v < .98 * speed)         // if we're 2% too slow,
  137.             vc = 1.1 * speed;          // accelerate hard.
  138.          else                               // if we are very close to speed,
  139.             vc = .5 * (s.v + speed);   // approach the speed gently.
  140.          // approach the lane you want for the turn:
  141.          if(s.nex_rad > 0.0)
  142.             lane = DIST_FROM_INSIDE;
  143.          else
  144.             lane = width - DIST_FROM_INSIDE;
  145.       }
  146.    }
  147.    else  {     // This is when we are in a curve:  (seek correct speed)
  148.          // calculate vc to maintain speed in corner
  149.          vc = .5 * (s.v + speed)/cos(alpha);
  150.          // calculate distance to end of curve:
  151.          if(s.cur_rad > 0.0)
  152.             to_end = s.to_end * (s.cur_rad + DIST_FROM_INSIDE);
  153.          else
  154.             to_end = -s.to_end * (s.cur_rad - DIST_FROM_INSIDE);
  155.          // compute required braking distance and compare:
  156.          if(to_end <= CritDist(s.v, speed_next, BRK_CRV_ACC))  {
  157.             vc = s.v - BRK_CRV_SLIP;
  158.          }
  159.    }
  160.  
  161.    // During the acceleration portion of a straightaway, the lane variable
  162.    // is not changed by the code above.  Hence the code below changes it a
  163.    // little at a time until there is no car dead_ahead.  This code here has
  164.    // no affect at all in the turns, nor in the braking portion
  165.    // of the straight.
  166.    if(s.dead_ahead)             // Change the lane a little if someone's
  167.      if(s.to_lft > s.to_rgt)       // in your way.
  168.         lane -= DELTA_LANE;            // lane must be a static variable
  169.      else
  170.         lane += DELTA_LANE;
  171.  
  172.    result.vc = vc;   result.alpha = alpha;
  173.    return result;
  174. }
  175.